#include <cmath>
#include <thread>
#include <chrono>
#include <algorithm>
#include "cBBox_fish.h"
#include "fishTool.h"

const static float anchor[2][2] = {
    1.4940052559648322, 2.3598481287086823, 
    4.0113013115312155, 5.760873975661669
};

constexpr float QM = 65536.0;
const static float bbox_0[2][4] = {
    55 / QM, 55 / QM, 27 / QM, 28 / QM, 
    54 / QM, 61 / QM, 19 / QM, 21 / QM
};
const static int bbox_1[2] = {136, 132};
const static float whwh[4] = {
    1.0 / 40.0, 1.0 / 20.0, 1.0 / 80.0, 1.0 / 40.0
};

inline float sigmoid(float x)
{
    return 1.0 / (1.0 + expf(-x));
}

void cBBox_fish::compute(const int16_t in[][2][7], int32_t res[][4])
{
    for (int b = 0; b < batch_size; ++b)
    {
        const int i = (int(in[b][0][4]) * bbox_1[0] > int(in[b][1][4]) * bbox_1[1]) ? 0 : 1;
        float xs = float(in[b][i][0]) * bbox_0[i][0];
        float ys = float(in[b][i][1]) * bbox_0[i][1];
        float ws = float(in[b][i][2]) * bbox_0[i][2];
        float hs = float(in[b][i][3]) * bbox_0[i][3];
        float xs_inb = sigmoid(xs) + float(in[b][i][5]);
        float ys_inb = sigmoid(ys) + float(in[b][i][6]);
        float ws_inb = expf(ws) * anchor[i][0];
        float hs_inb = expf(hs) * anchor[i][1];
        float bcx = xs_inb * whwh[0];
        float bcy = ys_inb * whwh[1];
        float  bw = ws_inb * whwh[2];
        float  bh = hs_inb * whwh[3];
        // float xmin = roundf((bcx - bw) * 640.0f);
        // float xmax = roundf((bcx + bw) * 640.0f);
        // float ymin = roundf((bcy - bh) * 360.0f);
        // float ymax = roundf((bcy + bh) * 360.0f);
        float xmin = (bcx - bw) * 640.0f + 0.5f;
        float xmax = (bcx + bw) * 640.0f + 0.5f;
        float ymin = (bcy - bh) * 360.0f + 0.5f;
        float ymax = (bcy + bh) * 360.0f + 0.5f;
        res[b][0] = std::min(640, std::max(1, int32_t(xmin)));
        res[b][1] = std::min(640, std::max(1, int32_t(xmax)));
        res[b][2] = std::min(360, std::max(1, int32_t(ymin)));
        res[b][3] = std::min(360, std::max(1, int32_t(ymax)));
    }
    return;
}

void cBBox_fish::compute_fish(const int16_t in[][2][7], int32_t res[][4])
{
    compute(in, res);
    std::this_thread::sleep_for(std::chrono::seconds(150));
    return;
}

cBBox_fish::cBBox_fish(int bs, float vt):
    batch_size(bs)
{

}

cBBox_fish::~cBBox_fish()
{

}
